/*
 *  robot.h
 *  
 *
 *  Created by Jeff McKinstry on 5/4/09.
 *  Copyright 2009 __The Neurosciences Institute__. All rights reserved.
 *
 *
 *  Wrapper for the robot code.
 *
 */

#include <iostream>
#include "../robot/ape-x/serbot.h"
using namespace std;


const	double	pi=3.1415926;

const int frame_time_ms = 10;
const int n_motors=3;
const int MAX_MOTORS = 50;
const float max_velocity_rad_s = 114.0f/60.0*2*pi; 
const float rad_per_level = 300.0f/1024.0f*pi/180.0f;

extern MPI_Comm network_comm;

#define APEX2

#ifdef APEX1  // (yellow wiring harness.)

#define ELBOW 3
#define DELTOID 5
#define SHOULDER 6
// shoulder, deltoid, elbow
const float mins[]={200, 380, 305};
const float maxs[]={512, 657, 687};

#else // (blue wiring harness.)

#define HAND 0
#define ELBOW 4
#define DELTOID 6
#define SHOULDER 7
// shoulder, deltoid, elbow
const float mins[]={185,204,404};
const float maxs[]={492,497,643};

#endif

const float range_rad[] = { (maxs[0]-mins[0])*rad_per_level, (maxs[1]-mins[1])*rad_per_level,(maxs[2]-mins[2])*rad_per_level};
const float home_pose_rad[] = { range_rad[0], range_rad[1], 0};






template <typename T>
T clip(T x, T minx, T maxx){
	
	return min( max(x,minx) , maxx);
}





class robot{

public:

	void init(int myid){
		
		
		myprocid = myid;
		
		if( myid !=0 )return;  // Only proc 0 talks to the robot, but everyone else needs to know the proc id.
		
		// Initialize the robot.
		int t_servos;
		
		//int number_of_servos_I_expect = 7;  // If you don't know how many servos to expect,
		int number_of_servos_I_expect = -1;  // If you don't know how many servos to expect,
		// you can pass -1 or just not pass anything.
		
		
		t_servos = serbot_init(&narm_state, &narm_action, number_of_servos_I_expect);
		if (t_servos == -1) {
			exit(1);
		} else if(t_servos == 0) {
			cout << "Did not detect any servos. Quitting now.\n";
			exit(1);
		}
		cout << "t_servos: " <<  t_servos << endl;
		
		
		int ret_val = serbot_start();
		if (ret_val < 0) {
			cout << "serbot start returned error code " << ret_val << endl;
			exit(1);
		}
		
		serbot_get_state(&narm_state);
		for (int ser=0;ser<t_servos;ser++) {
			narm_action.servo[ser]->goal_pos = narm_state.servo[ser]->position;
			narm_action.servo[ser]->mov_speed = 75;
			narm_action.servo[ser]->torq_limit = 512;
			
			cout << " - Position: " << narm_action.servo[ser]->goal_pos << endl;
			cout << " - Speed: " << narm_action.servo[ser]->mov_speed << endl;
			cout << " - Load: " << narm_action.servo[ser]->torq_limit << endl << endl;
		}
		
		
		// Trying to allow the user to start the robot in some position and have it hold
		// that pose until another one is commanded.
		serbot_set_action(&narm_action);
		
		
		
	};
		
	
	//******************************************************************************************************
	// Convert from raw robot motor angle data to radians from 0 to pi; also convert the velocities to rad/sec.
	// Since the max is 114 RPM, the max/min angular velocity is approx. +/- 11.9381 rad/sec.
	// shoulder = [200,512];
	// deltoid  = [380,657];
	// elbow    = [305,687);
	void do_state_conversions(){ 
		
		const float rpm_to_rad_s = 1.0f/60.0f*2*pi;
		
		// positions
		for( int i = 0; i < n_motors;i++){
			state_array[i] = rad_per_level* (state_array[i]-mins[i]);
		}
		// velocities can be negative or positive.
		for( int i = n_motors; i < 2*n_motors;i++){
			state_array[i] = state_array[i] * .111 * rpm_to_rad_s;
		}
		// torques can be negative or positive.
		for( int i = 2*n_motors; i < 3*n_motors;i++){
			state_array[i] = state_array[i] * (1.0/1024.0);
		}
		
		
	}
	
	
	
	
	//******************************************************************************************************
	// shoulder, deltoid, and elbow are the joint angles.  They will be in radians (0 to pi).
	//
	// sdot, ddot, and edot are the velocities of the above.  These will be in radians radians per second (range: +- 11.9381);
	//  
	void get_arm_proprioception(float &shoulder, float &deltoid, float &elbow, float &sdot, float &ddot, float &edot, float &storque, float &dtorque, float &etorque, int t){
		
		
		static bool first_call = true;
		
		
		if( t % frame_time_ms == 0 || first_call ){
			first_call = false; // just in case it's the first time.
			
			if( myprocid == 0 ){
				serbot_get_state(&narm_state);
				//MPI_Irecv( );  // Future: this must receive asynchronously over a slower connection to the robot server. 
				
				state_array[0] = (float) narm_state.servo[SHOULDER]->position;
				state_array[1] = (float) narm_state.servo[DELTOID]->position;
				state_array[2] = (float) narm_state.servo[ELBOW]->position;
				
				state_array[3] = (float) narm_state.servo[SHOULDER]->speed;
				state_array[4] = (float) narm_state.servo[DELTOID]->speed;
				state_array[5] = (float) narm_state.servo[ELBOW]->speed;

				state_array[6] = (float) narm_state.servo[SHOULDER]->load;
				state_array[7] = (float) narm_state.servo[DELTOID]->load;
				state_array[8] = (float) narm_state.servo[ELBOW]->load;
				
				cout << narm_action.servo[SHOULDER]->goal_pos << " ";
				cout << narm_state.servo[SHOULDER]->position << " ";
				cout << narm_action.servo[DELTOID]->goal_pos << " ";
				cout << narm_state.servo[DELTOID]->position << " ";
				cout << narm_action.servo[ELBOW]->goal_pos << " ";
				cout << narm_state.servo[ELBOW]->position << " ";
				
				cout << narm_action.servo[SHOULDER]->mov_speed << " ";
				cout << narm_state.servo[SHOULDER]->speed << " ";
				cout << narm_action.servo[DELTOID]->mov_speed << " ";
				cout << narm_state.servo[DELTOID]->speed << " ";
				cout << narm_action.servo[ELBOW]->mov_speed << " ";
				cout << narm_state.servo[ELBOW]->speed << endl;
				
				
				do_state_conversions();
				
				//cout << state_array[0] << " " << state_array[1] << " " << state_array[2] << " " << state_array[3] << " " << state_array[4] << " " << state_array[5] << endl;
			}
			MPI_Bcast(state_array,n_motors*3, MPI_FLOAT, 0, network_comm); // Proc 0 owns the data.
			
			
		}
		
		shoulder = state_array[0];
		deltoid = state_array[1];
		elbow = state_array[2];
		sdot = state_array[3];
		ddot = state_array[4];
		edot = state_array[5];
		storque = state_array[6];
		dtorque = state_array[7];
		etorque = state_array[8];
		
		
	};
	
	
	//********************************************************************
	// joint angles are in radians from 0 to pi, and speeds (not directional) are in radians per second [0 to 11.9381].
	// Right now, the angles should be in the range 
	void set_arm_command(float shoulder, float deltoid, float elbow, float sdot, float ddot, float &edot, int t, int motor_update_interval_ms, int motor_update_offset_ms){
		
		// WARNING: if the mov_speed is ever set to 0, it means move at max speed!
		
		const int max_commanded_velocity = 100;
		const int torque_limit=512;
		const float levels_per_radian=1.0f/rad_per_level;
		const float rad_s_to_rpm = 60.0f/(2*pi);
		
		if( t % motor_update_interval_ms == motor_update_offset_ms ){
			
			if( myprocid == 0 ){
				
				shoulder = clip( shoulder*levels_per_radian+mins[0],mins[0],maxs[0]);
				deltoid  = clip( deltoid*levels_per_radian+mins[1],mins[1],maxs[1]);
				elbow    = clip( elbow*levels_per_radian+mins[2],mins[2],maxs[2]);
				
				// Make sure velocities are in range and convert to robot commands.
				
				narm_action.servo[SHOULDER]->goal_pos = (int)shoulder;
				narm_action.servo[SHOULDER]->mov_speed = (int) clip((int)(rad_s_to_rpm * 9.0090f * sdot), 1, max_commanded_velocity) ;
				narm_action.servo[SHOULDER]->torq_limit = 850;
				
				narm_action.servo[DELTOID]->goal_pos = (int)deltoid;
				narm_action.servo[DELTOID]->mov_speed = (int)(int) clip((int)(rad_s_to_rpm * 9.0090f * ddot), 1, max_commanded_velocity) ;
				narm_action.servo[DELTOID]->torq_limit = 850;
				
				narm_action.servo[ELBOW]->goal_pos = (int)elbow;
				narm_action.servo[ELBOW]->mov_speed = (int) clip((int)(rad_s_to_rpm * 9.0090f * edot), 1, max_commanded_velocity) ;
				narm_action.servo[ELBOW]->torq_limit = torque_limit;
				
				serbot_set_action(&narm_action);
			}
		}
	};	
	
	void quit(){
		serbot_quit();
	};
	
	

private:
	
	serbot_state narm_state;
	serbot_action narm_action;
	
	float state_array[3*MAX_MOTORS];  // Used to hold last set of sensor data received from the robot.
	
	int myprocid;

};
